//
//  ev3.c
//  ev3-c-api
//  (C) 2014 Codesign
//  distributed under LGPL
//

// This file contains a simple example of how to control the robot.

#include "ev3.h"

#define ir_port             in_1
#define accel_port          in_2
#define compas_port         in_3
#define ultrasonic_port     in_4

void ev3_spin_to_heading(ev3_vehicle_t vehicle, int heading2, void(^callback)(int)) {
    ev3_read_si(vehicle->conn, 0, 0, compas_port, 0, ^(float val) {
        int heading1 = val * 2;
        int diff = heading2 - heading1;
        while (diff < -180) diff += 360;
        while (diff > 180) diff -= 360;
        printf("# %d -> %d (%d)\n", heading1, heading2, diff);
        ev3_vehicle_spin(vehicle, diff > 0 ? 10 : -10, abs(diff), ^(int d1, int d2) {
            ev3_read_si(vehicle->conn, 0, 0, compas_port, 0, ^(float val) {
                callback(val * 2);
            });
        });
    });
}

int main(int argc, char** argv) {
    struct ev3_connection* conn = ev3_connection_open("/dev/tty.EV3-SerialPort"); // the EV3's bluetooth port on my Mac
    if (conn->fd == -1) { printf("couldn't connect to the EV3 brick"); ev3_connection_close(conn); return 1; }
    ev3_vehicle_t vehicle = ev3_vehicle_create(190, 33, conn, out_c, out_b); // the gauge and the wheel diameter of my robot
    
    /*ev3_spin_to_heading(vehicle, 270, ^(int heading) {
        printf("spinned: %d\n", heading);
        ev3_finish();
    }); //*/
    
    /*ev3_vehicle_spin(vehicle, 10, 20, ^(int d1, int d2) {
        ev3_finish();
//        ev3_vehicle_move(vehicle, 10, 1000, ^{
//            __block int should_stop = 0;
//            ev3_read_si(conn, 0, 0, in_1, 0, ^(float val) { if (val < 20) should_stop = 1; });
//            ev3_read_si(conn, 0, 0, in_4, 0, ^(float val) { if (val < 20) should_stop = 1; });
//            return should_stop;
//        }, ^(int d1, int d2) {
//            printf("moved: %d %d\n", d1, d2);
//            ev3_vehicle_spin(vehicle, 10, 45, ^(int d1, int d2) {
//                printf("spinned: %d %d\n", d1, d2);
//                ev3_finish();
//            });
//        });
    }); //*/

    /*ev3_read_si(conn, 0, 0, in_1, 0, ^(float val) { printf("input port 1: %f\n", val); });
    ev3_read_si(conn, 0, 0, in_2, 0, ^(float val) { printf("input port 2: %f\n", val); });
    ev3_read_si(conn, 0, 0, in_3, 0, ^(float val) { printf("input port 3: %f\n", val); });
    ev3_read_si(conn, 0, 0, in_4, 0, ^(float val) { printf("input port 4: %f\n", val); }); //*/
    
    /*ev3_get_count(conn, 0, 0, motor_d, ^(int count) { printf("count: %d\n", count); });
    ev3_step_and_notify(conn, 0, 0, out_d, 5, 95, ^(int count) {
        printf("count: %d\n", count);
        ev3_finish();
    }); //*/
    
    ev3_vehicle_move(vehicle, 20, 1000, ^{
        __block int should_stop = 0;
        ev3_read_si(conn, 0, 0, in_1, 0, ^(float val) { if (val < 20) should_stop = 1; });
        ev3_read_si(conn, 0, 0, in_4, 0, ^(float val) { if (val < 20) should_stop = 1; });
        return should_stop;
    }, ^(int d1, int d2) {
        printf("moved: %d %d\n", d1, d2);
//        ev3_vehicle_spin(vehicle, 10, 45, ^(int d1, int d2) {
//            printf("spinned: %d %d\n", d1, d2);
            ev3_finish();
//        });
    }); //*/

    printf("entering ev3 loop\n");
    ev3_loop(^{
        printf("cleaning up resources\n");
        ev3_vehicle_close(vehicle);
        ev3_connection_close(conn);
    });
    return 0;
}
